/**
 * @file SURF_Homography
 * @brief SURF detector + descriptor + FLANN Matcher + FindHomography
 * @author A. Huaman
 */

#include <stdio.h>
#include <iostream>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/features2d.hpp"

#include "opencv2/opencv_modules.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/stitching/detail/autocalib.hpp"
#include "opencv2/stitching/detail/blenders.hpp"
#include "opencv2/stitching/detail/camera.hpp"
#include "opencv2/stitching/detail/exposure_compensate.hpp"
#include "opencv2/stitching/detail/matchers.hpp"
#include "opencv2/stitching/detail/motion_estimators.hpp"
#include "opencv2/stitching/detail/seam_finders.hpp"
#include "opencv2/stitching/detail/util.hpp"
#include "opencv2/stitching/detail/warpers.hpp"
#include "opencv2/stitching/warpers.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace std;
using namespace cv;
using namespace cv::detail;

/**
 * @function main
 * @brief Main function
 */
int main( int argc, char** argv )
{
//	Ptr<FeaturesFinder> finder;
//	vector<Mat> images(2);
//	vector<ImageFeatures> features(2);
//
//	//read images
//	//Mat image0=imread("image0.jpg");
//	//Mat image1=imread("image1.jpg");
//	Mat image0=imread("Cam1_0.jpg");
//	Mat image1=imread("Cam2_0.jpg");
//
//	images[0]=image0;
//	images[1]=image1;
//
//	finder=new SurfFeaturesFinder();
//
//	(*finder)(images[0],features[0]);
//	(*finder)(images[1],features[1]);
//
//	finder->collectGarbage();
//
//	//vector<MatchesInfo> pairwise_matches;
//	MatchesInfo pairwise_matches;
//	BestOf2NearestMatcher matcher(false,0.48f);
//
//	//matcher(features,pairwise_matches);
//	//matcher.operator ()(features,pairwise_matches);
//	matcher.operator ()(features[0],features[1],pairwise_matches);
//	matcher.collectGarbage();
//
//	//-- Draw matches
//	Mat img_matches;
//	drawMatches(image0,features[0].keypoints,image1,features[1].keypoints,pairwise_matches.matches,img_matches);
//
//	//-- Show detected matches
//	imshow("Matches", img_matches);

	// Use the Homography Matrix to warp the images
//	Mat result;

//	warpPerspective(image0,result,pairwise_matches.H,Size(image0.cols+image1.cols,image0.rows));
//	//warpPerspective(image0,result,pairwise_matches.H,Size(image0.cols,image0.rows));
//
//	imshow("image0",result);
//
//	Mat half(result,Rect(0,0,image0.cols,image0.rows));
//
//	image1.copyTo(half);
//
//	imshow( "Result", result );

//	Mat A1=(Mat_<double>(4,3)<<
//			1,0,-3.1704540944019521e+002,
//			0,1,-2.6384631465331626e+002,
//			0,0,0,
//			0,0,1);
//
//	Mat R=(Mat_<double>(4,4)<<
//			8.6392252347558840e-001, -6.1965732223294219e-002,4.9979808069019471e-001,0,
//			4.7341208631506072e-002,9.9799950726200048e-001, 4.1902189323618437e-002,0,
//			-5.0139473810250501e-001, -1.2539199928031493e-002,8.6512778539836832e-001,0,
//			0,0,0,1);
//
//	Mat T=(Mat_<double>(4,4)<<
//			1,0,0,9.3267064903523107e+001,
//			0,1,0,-9.7653300589483099e+000,
//			0,0,1,1.2597717497268791e+002,
//			0,0,0,1);
//
//	Mat A2=(Mat_<double>(3,4)<<
//			6.8175298327590724e+002, 0., 3.5906681856124834e+002, 0,
//			0., 6.8175298327590724e+002, 2.4335493682585457e+002, 0,
//			0., 0., 1., 0);
//
//	cout<<"det="<<determinant(R);
//
//	//Mat transf=(100*A2)*((100*T)*((100*R)*(100*A1)));
//	Mat transf=A2*(T*(R*A1));

	Mat src0=imread("left01.jpg");
	Mat src1=imread("right01.jpg");

	Size boardSize;
	boardSize.width=8;
	boardSize.height=6;

	bool found0,found1;
	vector<Point2f> corners0;
	vector<Point2f> corners1;

	found0 = findChessboardCorners(src0, boardSize, corners0,
						CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
	cout<<"found0="<<found0<<endl;

	found1 = findChessboardCorners(src1, boardSize, corners1,
							CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
	cout<<"found1="<<found1<<endl;

	Mat H = findHomography( corners0, corners1, CV_RANSAC );
	//Mat H = findHomography( corners0, corners1, CV_LMEDS  );

	Mat transf=H;

	Mat result,resImg;

	Mat image0=imread("Pair1_17_1.jpg");
	Mat image1=imread("Pair1_17_2.jpg");

	warpPerspective(image1,result,transf,Size(image0.cols+image1.cols,image0.rows));

//	Size s;
//	s.width=image0.cols+image1.cols;
//	s.height=image0.rows;
//	resize(result,resImg,s);

//	imshow("warp result",resImg);

	imshow("warp result",result);

	Mat half(result,Rect(0,0,image0.cols,image0.rows));

	image0.copyTo(half);

	imshow( "Result", result );

	waitKey(0);

	return 0;
}

